#include "stip.h"

STIP::Stip::Stip(double _sigmaSpace, double _sigmaTime, double _K, int _neighbourSpace, int _neighbourTime, float _thresholdStrength):sigmaSpace(_sigmaSpace),sigmaTime(_sigmaTime),K(_K),neighbourSpace(_neighbourSpace),neighbourTime(_neighbourTime),thresholdStrength(_thresholdStrength)
{
  gradientXImage = NULL ;
  gradientYImage = NULL ;
  stipStrengthImage = NULL ;
  gradientXXImage = NULL ;
  gradientYYImage = NULL ;
  gradientXYImage = NULL ;
  gradientXTimeImage = NULL ;
  gradientYTimeImage = NULL ;
  gradientTimeTimeImage = NULL ;
  gradientTimeImage = NULL ;
  aviFileReady = false ;
}
void STIP::Stip::setVideoName(const char * _aviFileName, const char * _stipPointsFileName)
{
  aviFileName = _aviFileName ;
  stipPointsFileName = _stipPointsFileName ;
}
void STIP::Stip::init()
{
  outFileStream.open(stipPointsFileName.c_str()) ;
  if( !outFileStream ) aviFileReady = false ;
  else aviFileReady = true ;

  CvCapture * capture = cvCaptureFromFile(aviFileName.c_str()) ;
  if( capture == NULL )
    {
      ERROR("avi file is not exist!") ;
      exit(-1) ;
    }
  int imageNum = 0 ;
  IplImage * frame = NULL ;
  while( true )
    {
      frame = cvQueryFrame(capture) ;
      if( frame == NULL ) break ;
      IplImage * imagePtr = cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
      if( frame->nChannels == 1 )
	cvCopyImage(frame,imagePtr) ;
      else 
	cvCvtColor(frame,imagePtr,CV_BGR2GRAY) ;
      cvSmooth(imagePtr,imagePtr,CV_GAUSSIAN,3,3) ;
      imageList.push_back(std::make_pair(imageNum++,imagePtr)) ;
    }
  INFO2("video frams:",imageList.size()) ;
  // 视频序列高斯平滑
  gaussianSmoothTimeList(imageList,neighbourTime,sigmaTime) ;
  // 计算时间序列上的梯度
  calculateTimeGradient(gaussianTimeImageList) ;
  if( imageList.size() > 10 )
    {
      IplImage * imagePtr = imageList.front().second ;
      gradientXImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1);
      gradientYImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1);
      gradientXXImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1);
      gradientYYImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1);
      gradientXYImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1);
      gradientXTimeImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1);
      gradientYTimeImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1);
      gradientTimeTimeImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1) ;
      gradientTimeImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1) ;
    }
}
void STIP::Stip::clear()
{
  while( imageList.size())
    {
      IplImage * ptr = imageList.front().second ;
      imageList.pop_front() ;
      cvReleaseImage(&ptr) ;
    }
  while( gaussianTimeImageList.size() )
    {
      IplImage * ptr = gaussianTimeImageList.front().second ;
      gaussianTimeImageList.pop_front() ;
      cvReleaseImage(&ptr) ;
    }
  while( gradientTimeImageList.size() )
    {
      IplImage* frame = gradientTimeImageList.front().second ;
      cvReleaseImage(&frame) ;
      gradientTimeImageList.pop_front() ;
    }
  if( outFileStream.is_open())
    outFileStream.close() ;
  if( gradientXImage != NULL )
    cvReleaseImage(&gradientXImage) ;
  if( gradientYImage != NULL )
    cvReleaseImage(&gradientYImage) ;
  if( stipStrengthImage != NULL )
    cvReleaseImage(&stipStrengthImage) ;
  if( gradientXXImage != NULL )
    cvReleaseImage(&gradientXXImage) ;
  if( gradientYYImage != NULL )
    cvReleaseImage(&gradientYYImage) ;
  if( gradientXYImage != NULL )
    cvReleaseImage(&gradientXYImage) ;
  if( gradientXTimeImage != NULL )
    cvReleaseImage(&gradientXTimeImage) ;
  if( gradientYTimeImage != NULL )
    cvReleaseImage(&gradientYTimeImage) ;
  if( gradientTimeTimeImage != NULL )
    cvReleaseImage(&gradientTimeTimeImage) ;
  if( gradientTimeImage != NULL )
    cvReleaseImage(&gradientTimeImage) ;
}
bool STIP::Stip::getReady()
{
  return aviFileReady ; 
}
STIP::Stip::~Stip()
{
  clear() ;
}
int STIP::Stip::indexRange(int index, int range)
{
  if( index >= 0 && index < range ) return index ;
  if( index < 0 ) return index*-1 ;
  if( index >= range ) return indexRange(2*(range-1)-index,range) ;
}
// 计算空间上的灰度图像
void STIP::Stip::calculateSpaceGradient(IplImage* imagePtr)
{
  if( gradientXImage == NULL )
    gradientXImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1) ;
  if( gradientYImage == NULL )
    gradientYImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1) ;
  cvZero(gradientXImage) ;
  cvZero(gradientYImage) ;

  uchar** rowPtr = new uchar*[imagePtr->height] ;
  for( int index_row = 0 ; index_row < imagePtr->height ; ++ index_row )
    rowPtr[index_row] = (uchar*)(imagePtr->imageData + index_row * imagePtr->widthStep) ;

  if( imagePtr->nChannels == 1 )
    {
      for( int index_row = 1 ; index_row < imagePtr->height - 1 ; ++ index_row )
	for( int index_col = 1 ; index_col < imagePtr->width -1 ; ++ index_col )
	  {
	    float gradientXValue = 0 , gradientYValue = 0;
	    uchar* ptrelem[3] ;
	    for( int index = 0 ; index < 3 ; ++ index )
	      ptrelem[index] = rowPtr[index_row-1+index] + index_col-1 ;
	    gradientXValue = -1.f*ptrelem[0][0] + 1.f*ptrelem[0][2] - 2.f*ptrelem[1][0] + 2.f*ptrelem[1][2] - 1.f*ptrelem[2][0] + 1.f*ptrelem[2][2] ;
	    gradientXValue *= 0.25f ; // scale to 1 
	    gradientYValue = -1.f*ptrelem[0][0] - 2.f*ptrelem[0][1] - 1.f*ptrelem[0][2] + 1.f*ptrelem[2][0] + 2.f*ptrelem[2][1] + 1.f*ptrelem[2][2] ;
	    gradientYValue *= 0.25f ; // scale to 1 ;
	    *((float*)(gradientXImage->imageData+index_row*gradientXImage->widthStep)+index_col) = fabs(gradientXValue) ;
	    *((float*)(gradientYImage->imageData+index_row*gradientYImage->widthStep)+index_col) = fabs(gradientYValue) ;
	  }
    }
  else
    {
      for( int index_row = 1 ; index_row < imagePtr->height - 1; ++ index_row )
	for( int index_col = 1 ; index_col < imagePtr->width - 1; ++ index_col )
	  {
	    float gradientXValue = 0 , maxGradientXValue = 0 ;
	    float gradientYValue = 0 , maxGradientYValue = 0 ; 
	    uchar * ptrelem[3] ;
	    for( int index = 0 ; index < 3 ; ++ index )
	      ptrelem[index] = rowPtr[index_row-1+index]+index_col*3-3 ;
	    for( int index = 0 ; index < 3 ; ++ index )
	      {
		gradientXValue = -1.f*ptrelem[0][0+index]+1.f*ptrelem[0][6+index] - 2.f*ptrelem[1][0+index] + 2.f*ptrelem[1][6+index] - 1.f*ptrelem[2][0+index] + 1.f*ptrelem[2][6+index] ;  
		gradientXValue = fabs(gradientXValue) ;
		if( gradientXValue > maxGradientXValue ) 
		  maxGradientXValue = gradientXValue ;

		gradientYValue = -1.f*ptrelem[0][0+index]-2.f*ptrelem[0][3+index]-1.f*ptrelem[0][6+index] + 1.f*ptrelem[2][0+index] + 2.f*ptrelem[2][3+index] + 1.f*ptrelem[2][6+index] ;
		gradientYValue = fabs(gradientYValue) ;
		if( gradientYValue > maxGradientYValue )
		  maxGradientYValue = gradientYValue ;
	      }
	    //maxGradientXValue *= 0.25f ;
	    //maxGradientYValue *= 0.25f ;
	    *((float*)(gradientXImage->imageData+index_row*gradientXImage->widthStep)+index_col) = maxGradientXValue ;
	    *((float*)(gradientYImage->imageData+index_row*gradientYImage->widthStep)+index_col) = maxGradientYValue ;
	  }
    }
  delete[] rowPtr ;
}
// 计算时间上的灰度
void STIP::Stip::calculateTimeGradient(std::list<std::pair<int,IplImage*> > &_gaussianTimeImageList) 
{
  if( _gaussianTimeImageList.size() < 5 ) return ; 
  listIterType indexIter = _gaussianTimeImageList.begin() ;
  for( int index =0  ; index < _gaussianTimeImageList.size() - 5 ; ++ index, ++ indexIter  )
    {
      listIterType iter = indexIter ;
      IplImage* gradientTime = cvCreateImage(cvGetSize(iter->second),IPL_DEPTH_32F,1) ;
      uchar* gradientData = (uchar*)gradientTime->imageData ;
      float* gradientPoint = NULL ;

      uchar* preImageData[2];
      preImageData[0] = (uchar*)((iter->second)->imageData) ;
      iter ++ ;
      preImageData[1] = (uchar*)((iter->second)->imageData) ;
      iter ++ ; 
      int imageNum = iter->first ;
      iter ++ ;
      uchar* nextImageData[2];
      nextImageData[1] = (uchar*)((iter->second)->imageData);
      iter ++ ;
      nextImageData[0] = (uchar*)((iter->second)->imageData);
  
      float* preImagePoint[2] ;
      float* nextImagePoint[2] ;
  
      int rows = iter->second->height ;
      int cols = iter->second->width ;
      int nChannels = iter->second->nChannels ;
      int widthStep = iter->second->widthStep ;
      
      if( nChannels == 1 )
	{
	  // 单通道
	  for( int index_row = 0 ; index_row < rows ; ++ index_row )
	    {
	      preImagePoint[0] = (float*)preImageData[0] ;
	      preImagePoint[1] = (float*)preImageData[1] ;
	      nextImagePoint[0] =(float*) nextImageData[0] ; 
	      nextImagePoint[1] =(float*)nextImageData[1] ;
	      gradientPoint= (float*)gradientData ;
	      for( int index_col = 0 ; index_col < cols ; ++ index_col ) 
		{
		  *((float*)gradientPoint) = fabs(float(*preImagePoint[0])-float(*nextImagePoint[0])+2.f*float(*preImagePoint[1])-2.f*float(*nextImagePoint[1])) ;
		  preImagePoint[0] += nChannels ;
		  nextImagePoint[0] += nChannels ;
		  preImagePoint[1] += nChannels ;
		  nextImagePoint[1] += nChannels ;
		  gradientPoint += nChannels ;
		}
	      preImageData[0] += widthStep ; 
	      nextImageData[0] += widthStep ;
	      preImageData[1] += widthStep ; 
	      nextImageData[1] += widthStep ;
	      gradientData += widthStep ;
	    }
	}
      else
	{
	  for( int index_row = 0 ; index_row < rows ; ++ index_row )
	    {
	      preImagePoint[0] = (float*)preImageData[0] ;
	      nextImagePoint[0]=(float*)nextImageData[0] ;
	      preImagePoint[1] =(float*)preImageData[1] ;
	      nextImagePoint[1]=(float*)nextImageData[1] ;
	      gradientPoint = (float*)gradientData ;
	      for( int index_col = 0 ; index_col < cols ; ++ index_col )
		{
		  float gradientValue = 0 , maxGradientValue = 0 ;
		  gradientValue = fabs(float(*preImagePoint[0])-float(*nextImagePoint[0])+2.f*float(*preImagePoint[1])-2.f*float(*nextImagePoint[1])) ;
		  if( gradientValue > maxGradientValue ) maxGradientValue = gradientValue ;
		  gradientValue = fabs(float(*(preImagePoint[0]+1))-float(*(nextImagePoint[0]+1))+2.f*float(*(preImagePoint[1]+1))-2.f*float(*(nextImagePoint[1]+1))) ;
		  if( gradientValue > maxGradientValue ) maxGradientValue = gradientValue ;
		  gradientValue = fabs(float(*(preImagePoint[0]+2))-float(*(nextImagePoint[0]+2))+2.f*float(*(preImagePoint[1]+2))-2.f*float(*(nextImagePoint[1]+2))) ;
		  if( gradientValue > maxGradientValue ) maxGradientValue = gradientValue ;
		  *((float*)gradientPoint) = maxGradientValue*0.25f ;
		  preImagePoint[0] += nChannels ;
		  preImagePoint[1] += nChannels ;
		  nextImagePoint[0] +=nChannels ;
		  nextImagePoint[1] +=nChannels ;
		  gradientPoint += 1 ;
		}
	      preImageData[0] += widthStep ;
	      preImageData[1] += widthStep ;
	      nextImageData[0] += widthStep ;
	      nextImageData[1] += widthStep ;
	      gradientData += gradientTime->widthStep ;
	    }
	}
      gradientTimeImageList.push_back(std::make_pair(imageNum,gradientTime));
    }
}
// 对空间，时间的梯度值进行高斯平滑
void STIP::Stip::gaussianGradient()
{
  static cv::Mat gaussianSpaceKernel = getGaussianKernel(neighbourSpace,sigmaSpace,CV_32F) ;
  cvMul(gradientXImage,gradientXImage,gradientXXImage) ;
  cvMul(gradientYImage,gradientYImage,gradientYYImage) ;
  cvMul(gradientXImage,gradientYImage,gradientXYImage) ;
  cvMul(gradientXImage,gradientTimeImage,gradientXTimeImage) ;
  cvMul(gradientYImage,gradientTimeImage,gradientYTimeImage) ;
  cvMul(gradientTimeImage,gradientTimeImage,gradientTimeTimeImage) ;
  
  gaussianSmooth(gradientXTimeImage,neighbourSpace,gaussianSpaceKernel) ;
  gaussianSmooth(gradientYTimeImage,neighbourSpace,gaussianSpaceKernel) ;
  gaussianSmooth(gradientTimeTimeImage,neighbourSpace,gaussianSpaceKernel);
  
  gaussianSmooth(gradientXXImage,neighbourSpace,gaussianSpaceKernel) ;
  gaussianSmooth(gradientYYImage,neighbourSpace,gaussianSpaceKernel) ;
  gaussianSmooth(gradientXYImage,neighbourSpace,gaussianSpaceKernel) ;
}
// 空间平滑
void STIP::Stip::gaussianSmooth(IplImage* imagePtr,int apture,cv::Mat gaussianKernel) 
{
  IplImage * bufferImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_32F,1) ; 
  
  float** rowPtr = new float*[imagePtr->height] ;
  for( int index = 0 ; index < imagePtr->height ; ++ index )
    rowPtr[index] = (float*)( imagePtr->imageData+index*imagePtr->widthStep ) ;
  float* gaussianData = (float*)gaussianKernel.data ;
  int rows = imagePtr->height ;
  int cols = imagePtr->width ;
  uchar * bufferData = (uchar*)bufferImage->imageData ;
  uchar * bufferPoint = NULL ;
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      bufferPoint = bufferData ;
      for( int index_col = 0 ; index_col  < cols ; ++ index_col )
	{
	  int up_row = index_row - (apture >> 1) ;
	  int left_col = index_col - (apture >> 1) ;
	  float sum = 0 ;
	  for( int row = 0 ; row < apture ; ++ row )
	    for( int col = 0 ; col < apture ; ++ col )
		sum += rowPtr[indexRange(up_row+row,rows)][indexRange(left_col+col,cols)] * gaussianData[row] * gaussianData[col] ;
	  *(float*)(bufferPoint) = sum ;
	  bufferPoint += sizeof(float) ;
	}
      bufferData += imagePtr->widthStep ;
    }
  cvCopyImage(bufferImage,imagePtr) ;
  cvReleaseImage(&bufferImage) ;
  delete []rowPtr ;
}
// time gaussian smooth 
void STIP::Stip::gaussianSmoothTimeList(std::list<std::pair<int,IplImage*> > &_imageList, int _neighbourTime, float _sigmaTime) 
{
  cv::Mat gaussianTimeKernel = getGaussianKernel(_neighbourTime,_sigmaTime,CV_32F);
  if( _imageList.size() < _neighbourTime ) 
    {
      ERROR("_imageList is not long for smooth!") ;
      exit(-1) ;
    }
  float* Kernel = (float*)gaussianTimeKernel.data ;
  listIterType indexIter = _imageList.begin() ;
  cv::Size imageSize = cvGetSize(_imageList.front().second) ;
  for( int index = 0 ; index < _imageList.size() - _neighbourTime ; ++ index, ++ indexIter )
    {
      listIterType iter = indexIter ;
      IplImage* smoothImage = cvCreateImage(imageSize,IPL_DEPTH_32F,1) ;
      cvZero(smoothImage) ;


      int imageNum = iter->first + (_neighbourTime>>1) ;
      for( int num = 0 ; num < _neighbourTime ; ++ num, ++ iter)
	{
	  uchar* smoothData = (uchar*)smoothImage->imageData ;
	  float* smoothPoint = NULL ;
	  uchar* imageData = (uchar*)iter->second->imageData ;
	  uchar* imagePoint = NULL ;
	  for( int index_row = 0 ; index_row < (*iter).second->height ; ++ index_row )
	    {
	      smoothPoint = (float*)smoothData ;
	      imagePoint = (uchar*)imageData ;
	      for( int index_col = 0 ;  index_col < (*iter).second->width ; ++ index_col )
		{
		  *smoothPoint += *imagePoint*Kernel[num];
		  smoothPoint += 1 ;
		  imagePoint += 1 ;
		}
	      smoothData += smoothImage->widthStep ;
	      imageData += iter->second->widthStep ;
	    }
	}
      gaussianTimeImageList.push_back(std::make_pair(imageNum,smoothImage));
    }
}
void STIP::Stip::gaussianSmoothGradientTimeList(listIterType& _listIter, int _neighbourTime, float _sigmaTime)
{
  cv::Mat gaussianTimeKernel = getGaussianKernel(_neighbourTime,_sigmaTime,CV_32F) ;
  listIterType iter = _listIter ;
  float* Kernel = (float*)gaussianTimeKernel.data ;
  listIterType  indexIter = _listIter ;
  cvZero(gradientTimeImage) ;
  float** rowPtr = new float*[gradientTimeImage->height];
  for( int index_row = 0 ; index_row < gradientTimeImage->height ; ++ index_row )
    rowPtr[index_row] = (float*)(gradientTimeImage->imageData+index_row*gradientTimeImage->widthStep);
  
  int rows = indexIter->second->height; 
  int cols = indexIter->second->width ;
  int nChannels = indexIter->second->nChannels ;
  int widthStep = indexIter->second->widthStep ;

  for( int num = 0 ; num < _neighbourTime ; ++ num, ++ indexIter)
    {
      uchar* imageData = (uchar*)indexIter->second->imageData ;
      float* imagePoint = NULL ;
      for( int index_row = 0 ; index_row < rows ; ++ index_row )
	{
	  imagePoint = (float*)imageData ;
	  for( int index_col = 0 ;  index_col < cols; ++ index_col )
	    {
	      rowPtr[index_row][index_col] += (*imagePoint) * (Kernel[num]);
	      imagePoint += nChannels ;
	    }
	  imageData += widthStep ;
	}
      
    }  
  delete[] rowPtr ;
}
void STIP::Stip::calculateStrength()
{
  uchar * xxImageData = (uchar*)gradientXXImage->imageData ;
  float * xxImagePoint = NULL ;
  uchar * yyImageData = (uchar*)gradientYYImage->imageData ;
  float * yyImagePoint = NULL ;
  uchar * xyImageData = (uchar*)gradientXYImage->imageData ;
  float * xyImagePoint = NULL ;
  uchar * xtimeImageData = (uchar*)gradientXTimeImage->imageData ;
  float * xtimeImagePoint = NULL ;
  uchar * ytimeImageData = (uchar*)gradientYTimeImage->imageData ;
  float * ytimeImagePoint = NULL ;
  uchar * timetimeImageData = (uchar*)gradientTimeTimeImage->imageData ;
  float * timetimeImagePoint = NULL ;

  if( stipStrengthImage == NULL )
    stipStrengthImage = cvCreateImage(cvGetSize(gradientXImage),IPL_DEPTH_32F,1) ;
  uchar * stipStrengthData = (uchar*)stipStrengthImage->imageData ;
  float * stipStrengthPoint = NULL ;
  int rows = stipStrengthImage->height ; 
  int cols = stipStrengthImage->width ; 
  int widthStep = stipStrengthImage->widthStep ;
  int nChannels = stipStrengthImage->nChannels ;
  for( int index_row = 0 ; index_row < rows ; ++ index_row ) 
    {
      xxImagePoint = (float*)xxImageData ;
      yyImagePoint = (float*)yyImageData ;
      xyImagePoint = (float*)xyImageData ;
      xtimeImagePoint = (float*)xtimeImageData ;
      ytimeImagePoint = (float*)ytimeImageData ;
      timetimeImagePoint = (float*)timetimeImageData ;

      stipStrengthPoint = (float*)stipStrengthData ;
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  float Dxy,Dxx,Dyy,Dxt,Dyt,Dtt;
	  Dxy = *xyImagePoint ;
	  Dxx = *xxImagePoint ;
	  Dyy = *yyImagePoint ;
	  Dxt = *xtimeImagePoint ;
	  Dyt = *ytimeImagePoint ;
	  Dtt = *timetimeImagePoint ;
	  *stipStrengthPoint = Dxx*Dyy*Dtt-Dxx*Dyt*Dyt-Dtt*Dxy*Dxy+Dxy*Dxt*Dyt+Dxt*Dxy*Dyt-Dxt*Dxt*Dyy - K*(Dxx+Dyy+Dtt)*(Dxx+Dyy+Dtt)*(Dxx+Dyy+Dtt);
	  /*
	  if( *stipStrengthPoint > 10 ) 
	  {
	    std::cout << *stipStrengthPoint << std::endl ;
	    std::cout << "det(M)" << (Dxx*Dyy*Dtt-Dxx*Dyt*Dyt-Dtt*Dxy*Dxy+Dxy*Dxt*Dyt+Dxt*Dxy*Dyt-Dxt*Dxt*Dyy) << std::endl ;
	    std::cout << "trace(M)" << ((Dxx+Dyy+Dtt)*(Dxx+Dyy+Dtt)*(Dxx+Dyy+Dtt)) << std::endl ;
	    std::cout << Dxx << " " << Dxy << " " << Dxt << std::endl ;
	    std::cout << Dxy << " " << Dyy << " " << Dyt << std::endl ;
	    std::cout << Dxt << " " << Dyt << " " << Dtt << std::endl ;
	    std::cout << "***************" << std::endl ;
	    std::cout << "det2" << (Dxx*Dyy-Dxy*Dxy) << std::endl ;
	  }
	  */
	  xxImagePoint += nChannels ;
	  yyImagePoint += nChannels ;
	  xyImagePoint += nChannels ;
	  xtimeImagePoint += nChannels ;
	  ytimeImagePoint += nChannels ;
	  timetimeImagePoint += nChannels ;
	  stipStrengthPoint += nChannels ; 
	}
      xxImageData += widthStep ;
      yyImageData += widthStep ;
      xyImageData += widthStep ;
      xtimeImageData += widthStep ;
      ytimeImageData += widthStep ;
      timetimeImageData += widthStep ;
      stipStrengthData += widthStep ;
    }
}
void STIP::Stip::calculatePoints(float _thresholdStrength,int width, int height)
{
  IplImage* maxStrengthImage = cvCreateImage(cvGetSize(stipStrengthImage),IPL_DEPTH_32F,1) ;
  char * maxStrengthData = maxStrengthImage->imageData ;
  float * maxStrengthPoint = NULL ;

  int rows = stipStrengthImage->height; 
  int cols = stipStrengthImage->width ; 
  int widthStep = stipStrengthImage->widthStep ;
  int nChannels = stipStrengthImage->nChannels ;
  float** rowPtr = new float*[rows] ;
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    rowPtr[index_row] = (float*)(stipStrengthImage->imageData + index_row*widthStep);
  float maxStrength = 0 ;
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      maxStrengthPoint = (float*)maxStrengthData ;
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  int up_row = index_row - (height>>1) ;
	  int left_col = index_col - (width>>1) ;
	  float max = -100000 ;
	  for( int index_x = 0 ; index_x < height ; ++ index_x )
	    for( int index_y = 0 ; index_y  < width ; ++ index_y )
	      if( max < rowPtr[indexRange(index_x+up_row,rows)][indexRange(index_y+left_col,cols)] )
		max = rowPtr[indexRange(index_x+up_row,rows)][indexRange(index_y+left_col,cols)] ;
	  *maxStrengthPoint = (max>0?max:0) ;
	  if( *maxStrengthPoint > maxStrength ) maxStrength  = *maxStrengthPoint ;
	  maxStrengthPoint += nChannels ;
	}
      maxStrengthData += widthStep ;
    }
  pointsList.clear() ;
  maxStrengthData = maxStrengthImage->imageData ;
  float threshold ;
  threshold = _thresholdStrength>0.1*maxStrength?_thresholdStrength:0.1*maxStrength;
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      maxStrengthPoint = (float*)maxStrengthData ;
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  if( rowPtr[index_row][index_col] == *maxStrengthPoint && *maxStrengthPoint > threshold ) 
	    pointsList.push_back(cv::Point(index_col,index_row)) ;
	  maxStrengthPoint += nChannels ;
	}
      maxStrengthData += widthStep ;
    }
  INFO2("pointsList.size()",pointsList.size()) ;
  cvReleaseImage(&maxStrengthImage) ;
  delete[] rowPtr ;
}
// 基于距离及时空兴趣相应值的兴趣点选取算法
void STIP::Stip::calculatePointsMy(float maxL, float aerf)
{
  int rows = stipStrengthImage->height; 
  int cols = stipStrengthImage->width ; 
  int widthStep = stipStrengthImage->widthStep ;
  int nChannels = stipStrengthImage->nChannels ;
  float** rowPtr = new float*[rows] ;
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    rowPtr[index_row] = (float*)(stipStrengthImage->imageData + index_row*widthStep);
  float maxStrength = 0 ;
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  if( maxStrength < rowPtr[index_row][index_col] )
	    maxStrength  = rowPtr[index_row][index_col] ;
	}
    }
  pointsList.clear() ;
  float threshold ;
  threshold = 0.3*maxStrength > 10000 ? 0.3*maxStrength : 10000 ;
  for( int index_row = 0 ; index_row < rows ; ++ index_row )
    {
      for( int index_col = 0 ; index_col < cols ; ++ index_col )
	{
	  if( rowPtr[index_row][index_col] > threshold ) 
	    {
	      pointsList.push_back(cv::Point(index_col,index_row)) ;
	    }
	}
    }
  for( int i = 0 ; i < pointsList.size() ; ++ i )
    for( int j = 0 ; j < pointsList.size() ; )
      {
	if( i == j ) { ++j ; continue ;}
	float R = rowPtr[pointsList[i].y][pointsList[i].x] ;
	float len = hypot(pointsList[i].x-pointsList[j].x,pointsList[i].y-pointsList[j].y) ;
	if( len > maxL ) { ++ j ; continue ; }
	float Rmin = (maxL-len)/maxL * aerf * R ;
	if( rowPtr[pointsList[j].y][pointsList[j].x] < Rmin )
	    pointsList.erase(pointsList.begin()+j) ;
	else ++ j ;
      }
  INFO2("pointsList.size()",pointsList.size()) ;
  delete[] rowPtr ;

}
void STIP::Stip::drawCircle(IplImage* _imagePtr, std::vector<cv::Point>& _pointsList)
{
  cv::Scalar color = cv::Scalar(255,255,255) ;
  int radius = 3, thickness = 2 ;
  if( _imagePtr == NULL )
    {
      ERROR("_imagePtr is NULL!") ;
      exit(-1) ;
    }
  cv::Mat imageMat(_imagePtr) ;
  for( std::vector<cv::Point>::iterator iter = _pointsList.begin() ; iter < _pointsList.end() ; ++ iter )
    {
      cv::circle(imageMat,*iter,radius,color,thickness) ;
    }
}
void STIP::Stip::outputPoint(std::ofstream& _outFileStream,std::vector<cv::Point>& _pointsList,int _framesNum)
{
  _outFileStream << _framesNum << "\t" ;
  _outFileStream << _pointsList.size() << "\t" ;
  for( int index = 0 ; index < _pointsList.size() ; ++ index )
    _outFileStream << "[ " << _pointsList[index].x << " " << _pointsList[index].y << " ]" << "\t" ;
  _outFileStream << std::endl ;
}
void STIP::Stip::calculateSTIP()
{
  /*
  IplImage * imagePtr = imageList.begin()->second ;
  static IplImage * xImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_8U,1);
  static IplImage * yImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_8U,1);
  static IplImage * timeImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_8U,1) ;
  static IplImage * xGaussianImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_8U,1);
  static IplImage * yGaussianImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_8U,1);
  static IplImage * timeGaussianImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_8U,1) ;
  static IplImage * strengthImage = cvCreateImage(cvGetSize(imagePtr),IPL_DEPTH_8U,1) ;
  cvZero(xImage) ;
  cvZero(yImage) ;
  cvZero(timeImage ) ;
  */
  init();
  if( imageList.size() < neighbourTime*2 )
    {
      ERROR("imageList is short!") ;
      exit(-1) ;
    }
  listIterType gradientTimeIter = gradientTimeImageList.begin() ;
  listIterType imageListIter = imageList.begin() ;
  while( imageListIter->first < gradientTimeIter->first + (neighbourTime>>1) )
    imageListIter ++ ;
  if( imageListIter->first != gradientTimeIter->first + (neighbourTime>>1) )
    {
      ERROR("can't match the gradientImageList imageList!n") ;
      exit(-1) ;
    }
  int maxIndex = gradientTimeImageList.size() - (neighbourTime>>1) ;
  for( int index = imageListIter->first ; index < maxIndex ; ++ index )
    {
      calculateSpaceGradient(imageListIter->second) ;
      gaussianSmoothGradientTimeList(gradientTimeIter,neighbourTime,sigmaTime) ;
      /*
      convertImage(gradientXImage,xImage) ;
      convertImage(gradientYImage,yImage) ;
      convertImage(gradientTimeImage,timeImage) ;

      cvShowImage("Dx",xImage) ;
      cvShowImage("Dy",yImage) ;
      cvShowImage("Dtime",timeImage) ;
      IplImage * frameTemp = cvCloneImage(imageListIter->second) ;
      */
      gaussianGradient();
      calculateStrength() ;
      calculatePoints(thresholdStrength,10,10) ;
      //calculatePointsMy(3,0.5) ;
      //      drawCircle(frameTemp,pointsList) ;
      if( pointsList.size() )
	outputPoint(outFileStream,pointsList,index) ;
      /*
      convertImage(gradientXImage,xGaussianImage) ;
      convertImage(gradientYImage,yGaussianImage) ;
      convertImage(gradientTimeImage,timeGaussianImage) ;
      cvShowImage("source",frameTemp) ;
      cvShowImage("DxGaussian",xGaussianImage) ;
      cvShowImage("DyGaussian",yGaussianImage) ;
      cvShowImage("DtimeGaussian",timeGaussianImage) ;
      */
      gradientTimeIter ++ ;
      imageListIter ++ ;
      /*
      char c = cvWaitKey(33) ;
      if( c == 27 ) break ;
      */
    }
  clear() ;
}
